
#include "prog_usartmenu.h"

/**
 * can-ctrl
 * Program: prog_usartmenu
 */

uint32_t prog_usartmenu_lastcheck = 0;


int8_t prog_usartmenu_init(void)
{
	register_slot(prog_usartmenu_uartchar, uart_char_event);
	return 0;
}
int8_t prog_usartmenu_uninit(void)
{
	unregister_slot(prog_usartmenu_uartchar, uart_char_event);
	return 0;
}
int8_t prog_usartmenu_run(void)
{
	if(time() - prog_usartmenu_lastcheck > 1000)
	{

		prog_usartmenu_lastcheck = time();
	}
	return 1;
}

void usart_menu()
{
    usart_writeln("");
    usart_writeln("");
    usart_writeln("");
    usart_writeln("USART MENU FOR CAN CONTROLLER");
	usart_writeln("1. Headlights on/off");
    usart_writeln("2. Left direction light on/off");
    usart_writeln("3. Right direction light on/off");
    usart_writeln("4. Servo 1 to position ->");
    usart_writeln("5. Servo 2 to position ->");
    usart_writeln("6. Read temperature");
    
}

uint8_t can_headlights;

void prog_usartmenu_uartchar(Event *ev)
{
	char *selection = (char *) ev->data;
    usart_echo = 0;
    sound(1000,200);
    switch (*selection)
    {
        case '1': //headlights on/off
            if(can_headlights)
			{ 
				can_message_headlights(CAN_MSG_HEADLIGHTS_OFF); 
				can_headlights =0;
			}
            else
			{ 
				can_message_headlights(CAN_MSG_HEADLIGHTS_ON);
				can_headlights = 1;
			}
            break;
        case '2': //Left direction light on/off
			can_message_directionlights(1);
            break;
        case '3': //Right direction light on/off
            can_message_directionlights(2);
            break;
        case '4': //Servo 1 to position
			unregister_slot(prog_usartmenu_uartchar, uart_char_event);
            can_message_servo_position(1, usart_read_servo());
			register_slot(prog_usartmenu_uartchar, uart_char_event);
            break;
        case '5': //Servo 2 to position
            unregister_slot(prog_usartmenu_uartchar, uart_char_event);
            can_message_servo_position(2, usart_read_servo());
			register_slot(prog_usartmenu_uartchar, uart_char_event);
            break;
        case '6': //Read temperature
            usart_print_temperature(can_message_get_temperature());
            break;
		case 'm':
		case 'M':
		case '?':
				usart_menu();
				break;
        default:
            usart_writeln("Wrong selection, please try again");
            usart_menu();
            break;
    }
}

void usart_print_temperature(int16_t temperature)
{
    usart_write("Temperature is: ");
    char temp[2];
    sprintf(temp, "%i", temperature);
    usart_writeln(temp);
}

uint16_t usart_read_servo(void)
{
    char* temp;
    usart_write("Give servo position: ");
    temp = usart_readln();
    return atoi(temp);
}